#pragma once

struct float4
{
	float x,y,z,w;
	float4(){}
	float4(float _x,float _y,float _z, float _w):x(_x),y(_y),z(_z),w(_w){}
	float4(const float3& v,float iw):x(v.x),y(v.y),z(v.z),w(iw){}
	void set(float ix,float iy, float iz,float iw){x=ix;y=iy;z=iz;w=iw;}

	float& operator[](int index){return *(&x+index);}
	const float& operator[] (int index) const{return *(&x+index);}

	inl float4 operator+(const float4& v) const
	{
		return float4(x+v.x,y+v.y,z+v.z,w+v.w);
	}
	inl float4 operator-(const float4& v) const
	{
		return float4(x-v.x,y-v.y,z-v.z,w-v.w);
	}
	inl float4 operator*(float s) const
	{
		return float4(x*s,y*s,z*s,w*s);
	}
	inl float4 operator*(const float4& v) const
	{
		return float4(x*v.x,y*v.y,z*v.z,w*v.w);
	}
	inl friend float4 operator*(float s, const float4&v)
	{
		return float4(v.x*s,v.y*s,v.z*s,v.w*s);
	}
	inl float4 operator/(float s) const
	{
		return float4(x/s,y/s,z/s,w/s);
	}
	inl void operator+=(const float4& v)
	{
		x+=v.x;y+=v.y;z+=v.z;w+=v.w;
	}
	inl void operator-=(const float4& v)
	{
		x-=v.x;y-=v.y;z-=v.z;w-=v.w;
	}
	inl void operator*=(float s)
	{
		x*=s; y*=s;z*=s;w*=s;
	}
	inl void operator/=(float s)
	{
		x/=s; y/=s;z/=s;w/=s;
	}

	inl friend float dot(const float4& v1, const float4& v2)
	{
		return v1.x*v2.x+v1.y*v2.y+v1.z*v2.z+v1.w*v2.w;
	}

	inl friend float length(const float4 v)
	{
		return sqrtf(dot(v,v));
	}

/*
	inl friend float plane_distance(const float4& plane, const float3& pos)
	{
		return dot(*((const float3*)&plane), pos)+plane.w;
	}
*/

	inl void normalize()
	{
		float l=length(*this);
		x/=l;
		y/=l;
		z/=l;
		w/=l;
	}

	inl float4 normalize() const
	{
		float l=length(*this);
		return float4(x/l,y/l,z/l,w/l);
	}

};

inl float4 lerp(const float4& p0, const float4& p1, float weight)
{
	return (p1*weight)+(p0*(1.0f-weight));
}

typedef float4 quat;
inline quat slerp( const quat& pStart, const quat& pEnd, float pFactor)
{
	// calc cosine theta
	float cosom = pStart.x * pEnd.x + pStart.y * pEnd.y + pStart.z * pEnd.z + pStart.w * pEnd.w;

	// adjust signs (if necessary)
	quat end = pEnd;
	if( cosom < 0.0f)
	{
		cosom = -cosom;
		end.x = -end.x;   // Reverse all signs
		end.y = -end.y;
		end.z = -end.z;
		end.w = -end.w;
	} 

	// Calculate coefficients
	float sclp, sclq;
	if( (1.0f - cosom) > 0.0001f) // 0.0001 -> some epsillon
	{
		// Standard case (slerp)
		float omega, sinom;
		omega = acos( cosom); // extract theta from dot product's cos theta
		sinom = sin( omega);
		sclp  = sin( (1.0f - pFactor) * omega) / sinom;
		sclq  = sin( pFactor * omega) / sinom;
	} else
	{
		// Very close, do linear interp (because it's faster)
		sclp = 1.0f - pFactor;
		sclq = pFactor;
	}

	return quat(
	sclp * pStart.x + sclq * end.x,
	sclp * pStart.y + sclq * end.y,
	sclp * pStart.z + sclq * end.z,
	sclp * pStart.w + sclq * end.w);
}




struct coloru8
{
	union
	{
		struct
		{
			uint8 r,g,b,a;
		};

		uint32 rgba;
	};

	coloru8(){}
	coloru8(uint32 _rgba):rgba(_rgba){}
	coloru8(uint32 _a,uint32 _r,uint32 _g,uint32 _b):a(_a),r(_r),g(_g),b(_b){}

	coloru8(const float4& float_color)
	{
		set( (uint8)clamp(float_color.x*255.0f, 0.0f, 255.0f), (uint8)clamp(float_color.y*255.0f, 0.0f, 255.0f), (uint8)clamp(float_color.z*255.0f, 0.0f, 255.0f), (uint8)clamp(float_color.w*255.0f, 0.0f, 255.0f));
	}

	inl void set(uint8 i_r, uint8 i_g, uint8 i_b, uint8 i_a)
	{
		rgba=(i_r) | (i_g<<8) | (i_b<<16) | (i_a<<24);
	}

};

typedef float4 color4f;